/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/publicInterfaceSrc/navigator.cpp
 * @Description  : 导航的ROS节点入口程序。
 *                 程序初始化了ROS节点，并启动多个线程以处理导航、状态发布和外围设备通信等任务。
 *                 主要功能包括地图加载、运动控制参数初始化、任务回调处理、以及状态和命令的发布。
 *                 程序在启动所有必要的线程后，等待这些线程的完成以确保程序的正常运行。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:20:37
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "rclcpp/rclcpp.hpp"
#include "public_kernel.h" 
#include <thread>
#include <sys/resource.h>
#include "operation_peripheral.h" // for peripheral
#include "mark_check.h"
#include "operation_turn_around_omni.h"
// #include "parameters.hpp"

int main(int argc, char *argv[])
{
    auto& manager = NodeManager::getInstance();
    manager.initialize(argc, argv);
    auto node = manager.getNode();
    // agv_navigate::NodeParameters::declareParameters(node.get());

    setlocale(LC_ALL, "");
    // #1 load map once
    LoadMap();
    
    // #2 加载运动控制参数、初始化操作模式等
    InitNodeMotionParam();

    // #3 start two Threads for task call back; sub odom ;agv_info ; event
    std::thread publish_nav_vel_thread(PublishNavVelAndStatusThread, node);    // while
    std::thread publish_auto_status_thread(AutoStatusThread, node);            // while
    std::thread task_nav_thread(TaskAndSubNavInfoThread, node);                // multiThreadSpinner 12
    std::thread init_sub_peripheral_thread(OpClient::InitAcitonClient, node);  // wait to server
    std::thread pub_online_state_thread(PubOnlineStateThreadFun, node);        // while
    std::thread pub_steer_cmd_thread(PublishSteerCmdThread, node);             // while

    DEBUG_OUT("Thead id:" << std::this_thread::get_id());
    // #4 open timer task
    InitAutoTaskTimer();

    std::thread report_thread(InitReportThread);                               // InitLaserDetectThread(&nh);

    // wait sub Thread exit
    task_nav_thread.join();
    init_sub_peripheral_thread.join();
    pub_online_state_thread.join();
    publish_nav_vel_thread.join();
    publish_auto_status_thread.join();
    report_thread.join();
    pub_steer_cmd_thread.join();
    
    return 0;
}
